#include <motion_controller/bridge/planner_bridge.h>

// 构造函数
PlannerBridge::PlannerBridge(tf2_ros::Buffer* tf_buffer):
    __map_ptr(nullptr),__global_planner_ptr(nullptr), __local_planner_ptr(nullptr){
    // 全局路径规划器
    int global_planner_type = ros::param::param<int>("/global_planner/planner_type", 0);
    switch(global_planner_type){
        case 0:
            __global_planner_type = GlobalPlannerType::A_STAR;
            __global_planner_ptr = boost::make_shared<AstarPlanner>();
            break;
        default:
            __global_planner_type = GlobalPlannerType::A_STAR;
            __global_planner_ptr = boost::make_shared<AstarPlanner>();
            break;
    }
    __global_planner_ptr->SetDebugMode(false);
    // 局部路径规划器
    int local_planner_type = ros::param::param<int>("/local_planner/planner_type", 0);
    switch(local_planner_type){
        case 0:
            __local_planner_type = LocalPlannerType::DWA;
            __local_planner_ptr = boost::make_shared<DWAPlanner>(tf_buffer);
            break;
        default:
            __local_planner_type = LocalPlannerType::DWA;
            __local_planner_ptr = boost::make_shared<DWAPlanner>(tf_buffer);
            break;
    }   

    __path_pub = __nh.advertise<nav_msgs::Path>("/global_path_origin", 1, true);
    __path_smooth_pub = __nh.advertise<nav_msgs::Path>("/global_path_smooth", 1, true);
    __start_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/global_start_pose", 1, true);
    __goal_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/global_goal_pose", 1, true);
    __lookahead_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 1, true);
    __expand_points_pub = __nh.advertise<nav_msgs::OccupancyGrid>("/global_expand_points", 1, true);
    __local_path_pub = __nh.advertise<nav_msgs::Path>("/local_path", 1, true);
    __speed_pub = __nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1, true);
    __map_erode_pub = __nh.advertise<nav_msgs::OccupancyGrid>("/global_map_erode", 1, true);
}

// 析构函数
PlannerBridge::~PlannerBridge(){


}


// 设置地图数据
void PlannerBridge::SetMap2D(Map2D& map_2d, boost::shared_ptr<const nav_msgs::OccupancyGrid> map_ptr){
    if(__global_planner_ptr){
        __global_planner_ptr->SetMap2D(map_2d);
        __map_ptr = map_ptr;

        // 发布地图数据
        __map_erode_pub.publish(*map_ptr);
    }
}

// 发布速度
void PlannerBridge::PublishTwist(Velocity2D cmd_vel){
    geometry_msgs::Twist twist_msg;
    twist_msg.linear.x = cmd_vel.vx;
    twist_msg.linear.y = cmd_vel.vy;
    twist_msg.angular.z = cmd_vel.vth;
    __speed_pub.publish(twist_msg);
}

// 发布空速
void PlannerBridge::PublishTwist(){
    geometry_msgs::Twist twist_msg;
    __speed_pub.publish(twist_msg);
}

// 发布位姿
void PlannerBridge::PublishPose(ros::Publisher& pub, Pose2D pose_2d, string frame_id){
    geometry_msgs::PoseStamped pose_stamp;
    pose_stamp.header.frame_id = frame_id;
    pose_stamp.header.stamp = ros::Time::now();
    pose_stamp.pose = msg_convert::Pose2D2Pose(pose_2d);
    pub.publish(pose_stamp);
}

// 发布原始路径
void PlannerBridge::PublishPath(ros::Publisher& pub, vector<Pose2D>& points, string frame_id){
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = frame_id;
    path_msg.header.stamp = ros::Time::now();
    for(auto& point : points){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header = path_msg.header;
        pose_stamp.pose = msg_convert::Pose2D2Pose(point);
        // cout << "--> [path_msg] point_2f (" << point_2f.x << ", " << point_2f.y << ")"<< endl;
        pose_stamp.pose.position.z = 0.05;
        path_msg.poses.emplace_back(pose_stamp);
    }
    pub.publish(path_msg);    
}

// 发布扩展点
void PlannerBridge::PublishExpendPointsMap(vector<Pixel2D>& expend_points){
    if(__expand_points_pub.getNumSubscribers() > 0 && __map_ptr){
        nav_msgs::OccupancyGrid grid_msg;
        grid_msg.header.frame_id = "map";
        grid_msg.header.stamp = ros::Time::now();
        grid_msg.info = __map_ptr->info;
        grid_msg.data.assign(__map_ptr->data.size(), 0);
        for(auto& point : expend_points){
            long index = point.y * __map_ptr->info.width + point.x;
            if(index < 0 || index >= __map_ptr->data.size())
                continue;
            grid_msg.data[index] = 30;
        }
        __expand_points_pub.publish(grid_msg);
    }   
}

// 路径方向计算
vector<Pose2D> PlannerBridge::GenPathDirection(vector<Pose2D>& points, Pose2D goal){
    vector<Pose2D> new_points;
    if(points.size() <= 2){
        new_points = points;
        return new_points;
    }

    for(int i=0; i<points.size()-1; i++){
        Pose2D curr_point = points[i];
        Pose2D next_point = points[i+1];
        curr_point.yaw = math_utils::AnglePose2D(curr_point, next_point);
        new_points.emplace_back(curr_point);
    }
    new_points.emplace_back(goal);
    return new_points;
}


// 生成路径
bool PlannerBridge::GenerateP2PPath(Pose2D start, Pose2D goal, vector<Pose2D>& points){
    PublishPose(__start_pose_pub, start);
    PublishPose(__goal_pose_pub, goal);
    if(__global_planner_ptr){
        ros::Time start_time = ros::Time::now();
        vector<Pixel2D> expend_points; 
        points.clear();
        ros::Time gen_time;
        if(__global_planner_ptr->GeneratePath(start, goal, points, expend_points)){
            ROS_INFO_STREAM("[RvizPosesCallback] GeneratePath successfully!"); 
            PublishPath(__path_pub, points);
            PublishExpendPointsMap(expend_points);
            gen_time = ros::Time::now();

            // 路径平滑
            vector<Pose2D> smooth_points; 
            __global_planner_ptr->PathSmooth(points, smooth_points);
            PublishPath(__path_smooth_pub, smooth_points);

            // 计算路径方向
            points = GenPathDirection(smooth_points, goal);

            //计算耗时
            double total_time_cost = (ros::Time::now() - start_time).toSec();
            double gen_time_cost = (gen_time - start_time).toSec();
            double smooth_time_cost = (ros::Time::now() - gen_time).toSec();
            ROS_INFO("[GlobalPlanner] gen path time cost=%0.3f, gen cost=%0.3f, smooth cost=%0.3f", 
                total_time_cost, gen_time_cost, smooth_time_cost);
            return true;
        }
        else
            ROS_WARN_STREAM("[RvizPosesCallback] GeneratePath failed!"); 
    }
    return false;
}

// 计算速度指令
bool PlannerBridge::ComputeTwist(Velocity2D& cmd_vel, LidarScan2D lidar_scan_2d, 
    Pose2D robot_pose, Pose2D goal_pose){
    return  __local_planner_ptr->ComputeTwist(cmd_vel, lidar_scan_2d, robot_pose, goal_pose);
}

// 获取机器人当前位姿
bool PlannerBridge::GetRobotPose(geometry_msgs::PoseStamped& global_pose){
    return __local_planner_ptr->GetRobotPose(global_pose);
}

// 获取TF坐标变换
bool PlannerBridge::GetTFPose(geometry_msgs::PoseStamped& global_pose, string father_frame, 
    string child_frame, ros::Time stamp){
    return __local_planner_ptr->GetTFPose(global_pose, father_frame, child_frame, stamp);
}

// 全局路径转局部路径
bool PlannerBridge::PathGlobal2Local(Pose2D robot_pose, vector<Pose2D> global_path, vector<Pose2D>& local_path, float dist_threshold){
    local_path = math_utils::PathGlobal2Local(robot_pose, global_path);
    PublishPath(__local_path_pub, local_path, "base_link");
    return true;
}

// 检测目标点是否到达  距离和角度
bool PlannerBridge::IsGoalReached(Pose2D robot_pose, Pose2D goal_pose, 
    float xy_tolerance, float yaw_tolerance){
    float dist = math_utils::DistancePose2D(goal_pose, robot_pose);
    float theta = math_utils::DifferenceAngle(goal_pose.yaw, robot_pose.yaw);
    if(dist < xy_tolerance && fabs(theta) < yaw_tolerance){
        return true;
    }
    return false;
}

// 检测目标点是否到达  距离判断
bool PlannerBridge::IsPositionReached(Pose2D robot_pose, Pose2D goal_pose, float xy_tolerance){
    float dist = math_utils::DistancePose2D(goal_pose, robot_pose);
    cout << "[IsPositionReached] dist: " << dist << endl;
    if(dist < xy_tolerance){
        return true;
    }
    return false;
}

// 检测目标点是否到达  角度判断
bool PlannerBridge::IsAngleReached(Pose2D robot_pose, Pose2D goal_pose, float yaw_tolerance){
    float theta = math_utils::DifferenceAngle(goal_pose.yaw, robot_pose.yaw);
    cout << "[IsAngleReached] theta: " << theta << endl;
    if(fabs(theta) < yaw_tolerance){
        return true;
    }
    return false;
}

// 获取离当前位置最近的前瞻点  简单的距离计算，后期优化
bool PlannerBridge::FindLookAheadPointByDistance(vector<Pose2D>& local_path, Pose2D curr, Pose2D& target_point, float lookahead_distance){
    if(local_path.empty())
        return false;
    Pose2D origin_data;
    for(size_t i = local_path.size()-1; i >=0 ; --i){
        // float dis = math_utils::DistancePose2D(local_path[i], curr);
        float dis = math_utils::DistancePose2D(local_path[i], origin_data);
        if(dis < lookahead_distance){
            target_point = local_path[i];
            // PublishPose(__lookahead_pose_pub, target_point, "base_link");
            Pose2D global_goal = math_utils::Pose2DLocal2Global(curr, target_point);
            PublishPose(__lookahead_pose_pub, global_goal);
            cout << "[FindLookAheadPointByDistance] target_point: " << target_point 
                << ", global_goal: " << global_goal << endl;
            return true;
        }
    }
    return false;
}


// 旋转到指定角度
bool PlannerBridge::RotateToAngle(Velocity2D& cmd_vel, Pose2D curr, Pose2D goal){
    __local_planner_ptr->RotateToGoal(cmd_vel, curr, goal);
}


